#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
import cv2
import numpy as np
from cv_bridge import CvBridge

class CameraPublisher(Node):
    def __init__(self):
        super().__init__("camera_publisher")
        self.camera_device = 0  # 必选：根据实际设备修改（0/1/2）
        self.target_fps = 20  # 降低帧率，提高稳定性
        self.target_width = 640
        self.target_height = 480
        self.expected_pixel_count = self.target_width * self.target_height * 3  # 三通道总像素数
        self.image_pub = self.create_publisher(Image, "camera/image", 3)  # 更小队列，避免积压
        
        # 摄像头基础配置（兼容异常设备）
        self.cap = cv2.VideoCapture(self.camera_device, cv2.CAP_V4L2)  # Linux
        # Windows替换：cv2.CAP_DSHOW；Mac替换：cv2.CAP_AVFOUNDATION
        if not self.cap.isOpened():
            self.get_logger().fatal(f"无法打开摄像头 {self.camera_device}！")
            rclpy.shutdown()
            return
        
        # 简化参数设置（避免部分摄像头不兼容）
        self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, self.target_width)
        self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, self.target_height)
        self.cap.set(cv2.CAP_PROP_FPS, self.target_fps)
        self.cap.set(cv2.CAP_PROP_BUFFERSIZE, 1)
        
        self.get_logger().info(f"摄像头启动：尝试输出 {self.target_width}x{self.target_height} @ {self.target_fps}fps")
        
        self.bridge = CvBridge()
        self.timer = self.create_timer(1/self.target_fps, self.publish_image)

    def publish_image(self):
        ret, frame = self.cap.read()
        if not ret:
            self.get_logger().warn_throttle(1.0, "读取帧失败")
            return
        
        # 核心修复1：强制校正图像维度（不管原始格式如何）
        if frame.size == self.expected_pixel_count:
            # 一维数据 → 正常三通道图像
            frame = frame.reshape((self.target_height, self.target_width, 3))
        elif frame.size == self.target_width * self.target_height:
            # 一维灰度数据 → 三通道图像
            frame = frame.reshape((self.target_height, self.target_width))
            frame = cv2.cvtColor(frame, cv2.COLOR_GRAY2BGR)
        else:
            # 其他异常尺寸 → 强制缩放为目标尺寸
            frame = cv2.resize(frame, (self.target_width, self.target_height))
            if len(frame.shape) == 2:
                frame = cv2.cvtColor(frame, cv2.COLOR_GRAY2BGR)
        
        # 确保最终格式是 (高, 宽, 3)
        assert frame.shape == (self.target_height, self.target_width, 3), f"图像格式异常：{frame.shape}"
        
        try:
            ros_image = self.bridge.cv2_to_imgmsg(frame, "bgr8")
            ros_image.header.stamp = self.get_clock().now().to_msg()
            ros_image.width = self.target_width
            ros_image.height = self.target_height
            self.image_pub.publish(ros_image)
        except Exception as e:
            self.get_logger().error_throttle(1.0, f"发布失败：{str(e)}")

    def destroy_node(self):
        self.cap.release()
        self.get_logger().info("摄像头已释放")
        super().destroy_node()

def main(args=None):
    rclpy.init(args=args)
    rclpy.logging.set_logger_level("camera_publisher", rclpy.logging.LoggingSeverity.INFO)
    node = CameraPublisher()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == "__main__":
    main()